#include "libfreenect.hpp"
#include <iostream>
#include <cmath>
#include <stdio.h>
#include <string.h>
#include <ctype.h>
#include <pthread.h>
#include <vector>
#include <opencv/cv.h>
#include <opencv/cvaux.h>
#include <opencv/cxcore.h>
#include "Mutex.h"

using namespace cv;
/**
* @class Device
*
* @brief Methods for getting Kinect data into OpenCV
*
*
* This class contains method for getting Kinect data into OpenCV matrices.
*
* @author Elisabeth Lindquist, elisabeth.lindquist@gmail.com
*
*/
class Device : public Freenect::FreenectDevice {
  public:
	Device(freenect_context *_ctx, int _index)
		: Freenect::FreenectDevice(_ctx, _index), m_buffer_depth(FREENECT_DEPTH_11BIT_SIZE),m_buffer_rgb(FREENECT_VIDEO_RGB_SIZE), m_gamma(2048), m_new_rgb_frame(false), m_new_depth_frame(false),
		  depthMat(Size(640,480),CV_16UC1), rgbMat(Size(640,480),CV_8UC3,Scalar(0)), ownMat(Size(640,480),CV_8UC3,Scalar(0))
	{
		/*for( unsigned int i = 0 ; i < 2048 ; i++) {
			float v = i/2048.0;
			v = std::pow(v, 3)* 6;
			m_gamma[i] = v*6*256;
		}*/
		for (unsigned int i=0; i<2048; i++){
            const float k1 = 1.1863;
            const float k2 = 2842.5;
            const float k3 = 0.1236;
            const float depth = k3 * tanf(i/k2 + k1);
            m_gamma[i] = depth;
        }
	}

	// Do not call directly even in child
	void VideoCallback(void* _rgb, uint32_t timestamp);
	// Do not call directly even in child
	void DepthCallback(void* _depth, uint32_t timestamp);

	bool getVideo(Mat& output);
	bool getDepth(Mat& output);



  private:
	std::vector<uint8_t> m_buffer_depth;
	std::vector<uint8_t> m_buffer_rgb;
	std::vector<uint16_t> m_gamma;
	Mutex m_rgb_mutex;
	Mutex m_depth_mutex;
	bool m_new_rgb_frame;
	bool m_new_depth_frame;
    Mat depthMat;
	Mat rgbMat;
	Mat ownMat;
};




